/*!
  @file motion_solver_ransac.h
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _MOTION_SOLVER_RANSAC_H_
#define _MOTION_SOLVER_RANSAC_H_

#include <boost/scoped_ptr.hpp>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include "mrover_vo/motion_solver.h"
#include "mrover_vo/solver_method.h"
#include "mrover_vo/solver_method_arun.h"

namespace vo {

/*! 
 *  @brief  Two point algorithm solver
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  PCL-based
 */

template <typename ValType>
class MotionSolverRANSAC : public MotionSolver {
 public:
  typedef boost::scoped_ptr<MotionSolverRANSAC> Ptr;

  typedef Eigen::Matrix<ValType, Eigen::Dynamic, Eigen::Dynamic> MatrixXT;
  typedef Eigen::Matrix<ValType, 3, 1> Vector3T;
  typedef Eigen::Transform<ValType, 3, Eigen::Affine> Affine3T;

  typedef std::vector<Affine3T, Eigen::aligned_allocator<Affine3T> > Poses;
  typedef boost::shared_ptr<Poses> PosesPtr;
  typedef typename Poses::iterator PosesItr;

  typedef typename SolverMethod<ValType>::Ptr SolverMethodPtr;

  using MotionSolver::compute;


  MotionSolverRANSAC() 
      : method_(new SolverMethodArun<ValType>)
  { 
    ros::NodeHandle priv_nh("~");
    priv_nh.param<int>("ransac_num_itr", num_itr_, 100);
    priv_nh.param<double>("ransac_thresh_dist", thresh_dist_, 10.0);
    priv_nh.param<double>("ransac_thresh_inlier_ratio", thresh_inlier_ratio_, 0.1);
  }

  ~MotionSolverRANSAC() { }


  virtual void compute();

  void setIterationNumber(const size_t num_itr) { num_itr_ = num_itr; }

  void setInlierRatio(const double ratio) { thresh_inlier_ratio_ = ratio; }

 private:
  virtual inline bool initCompute() {
    if (!MotionSolver::initCompute()) return false;
    if (matches_->size() < method_->getRequiredSampleSize()) return false;
    return true;
  }

  virtual void copyCameraParams() {
    convertCV2Eigen<float>(cam_params_->K[0], K);
    convertCV2Eigen<float>(cam_params_->wT[0].inv() * cam_params_->wT[1], lTr);
    method_->setCameraParams(K, lTr);
  }

  void initializeData();

  void computePoseWithRandomSampling(PosesPtr &pose);

  ValType computeScore(const Affine3T &iTt);

  //! computes reprojection error for the points specified by indices
  /*! Note that the reprojection error is expressed by pixel. */
  void computeReprojectionError(const Affine3T &iTt, std::vector<ValType> &errors);

  //! generates random sample indices in range of [0...num_max-1]
  void generateRandomIndices(std::vector<int> &indices, const size_t num_max);

  template <typename ValT>
  void convertCV2Eigen(const cv::Mat &m, MatrixXT &e) {
    e.resize(m.rows, m.cols);
    for (int i = 0; i < m.rows; ++i)
      for (int j = 0; j < m.cols; ++j)
        e(i, j) = m.at<ValT>(i, j);
  }

  template <typename ValT>
  void convertEigen2CV(const MatrixXT &e, cv::Mat &m) {
    m.resize(m.rows, m.cols);
    for (int i = 0; i < e.rows(); ++i)
      for (int j = 0; j < e.cols(); ++j)
        m.at<ValT>(i, j) = e(i, j);
  }

  SolverMethodPtr method_;

  MatrixXT K;
  MatrixXT lTr;

  // ransac parameters
  int num_itr_;
  double thresh_dist_;
  double thresh_inlier_ratio_;

  MatrixXT input_pts;
  MatrixXT target_pts;
};

}  // namespace vo

#endif  // _MOTION_SOLVER_RANSAC_H_
